#########################################################################################
# 作者：hehung
# 联系：1398660197@qq.com
# 微信：hehung95
# 描述：
#########################################################################################

import keyboard
import time
import uart_ctrl

STEP_ACCURARY = 0.1
MAX_VALUE = 12.5
MIN_VALUE = 2.5

# 必须运行在root权限下
class class_kb_ctrl():
    def __init__(self):
        self.g_steer = [7.5, 7.5, 7.5, 7.5, 7.5, 7.5]
        self.app_uart = uart_ctrl.class_uart()

    # 发送控制命令给舵机
    def send_command_to_servo(self):
        # data_str = 'START::0-%.2f:1-%.2f:2-%.2f:3-%.2f:4-%.2f:5-%.2f::END\r\n' % (self.g_steer[0], self.g_steer[1], self.g_steer[2], self.g_steer[3], self.g_steer[4], self.g_steer[5])
        # print ('Send Data: %s' % data_str.encode())
        # self.app_uart.send(data_str)
        self.app_uart.servo_ctrl(self.g_steer)

    def kb_ctrl_main(self):
        while True:
            if keyboard.is_pressed('esc'):
                # 按下ESC退出
                break
            else:
                if keyboard.is_pressed('down'): 
                    # 上
                    self.g_steer[1] += STEP_ACCURARY
                    if (self.g_steer[1] > MAX_VALUE):
                        self.g_steer[1] = MAX_VALUE
                if keyboard.is_pressed('up'):
                    # 下
                    self.g_steer[1] -= STEP_ACCURARY
                    if (self.g_steer[1] < MIN_VALUE):
                        self.g_steer[1] = MIN_VALUE
                if keyboard.is_pressed('right'):
                    # 左转
                    self.g_steer[0] -= STEP_ACCURARY
                    if (self.g_steer[0] < MIN_VALUE):
                        self.g_steer[0] = MIN_VALUE
                if keyboard.is_pressed('left'):
                    # 右转
                    self.g_steer[0] += STEP_ACCURARY
                    if (self.g_steer[0] > MAX_VALUE):
                        self.g_steer[0] = MAX_VALUE
                if keyboard.is_pressed('w'):
                    # 前
                    self.g_steer[2] += STEP_ACCURARY
                    if (self.g_steer[2] > MAX_VALUE):
                        self.g_steer[2] = MAX_VALUE
                    ######################
                    self.g_steer[3] += STEP_ACCURARY
                    if (self.g_steer[3] > MAX_VALUE):
                        self.g_steer[3] = MAX_VALUE
                if keyboard.is_pressed('s'):
                    # 后
                    self.g_steer[2] -= STEP_ACCURARY
                    if (self.g_steer[2] < MIN_VALUE):
                        self.g_steer[2] = MIN_VALUE
                    ######################
                    self.g_steer[3] -= STEP_ACCURARY
                    if (self.g_steer[3] < MIN_VALUE):
                        self.g_steer[3] = MIN_VALUE
                if keyboard.is_pressed('shift'):
                    self.g_steer[3] -= STEP_ACCURARY
                    if (self.g_steer[3] < MIN_VALUE):
                        self.g_steer[3] = MIN_VALUE
                if keyboard.is_pressed('ctrl'):
                    self.g_steer[3] += STEP_ACCURARY
                    if (self.g_steer[3] > MAX_VALUE):
                        self.g_steer[3] = MAX_VALUE
                if keyboard.is_pressed('d'):
                    # 爪子左旋转
                    self.g_steer[4] -= STEP_ACCURARY
                    if (self.g_steer[4] < MIN_VALUE):
                        self.g_steer[4] = MIN_VALUE
                if keyboard.is_pressed('a'):
                    # 爪子右旋转
                    self.g_steer[4] += STEP_ACCURARY
                    if (self.g_steer[4] > MAX_VALUE):
                        self.g_steer[4] = MAX_VALUE
                if keyboard.is_pressed('enter'):
                    # 抓握
                    # 按下enter抓握，松开enter释放
                    self.g_steer[5] += 0.1
                    if (self.g_steer[5] > MAX_VALUE):
                        self.g_steer[5] = MAX_VALUE
                else:
                    self.g_steer[5] = 7.5
                # value = keyboard.read_key()
                # print ('Recv key: ', value)
                self.send_command_to_servo()
                time.sleep(0.03)

